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ABSTRACT 


Unmanned Underwater Vehicles frequently rely on two-dimensional sensors for 
information about their surroundings. These sensors do not provide adequate information 
for obstacle avoidance in cluttered maritime environments. To address that issue, a three- 
dimensional reconstruction of the environment utilizing occupancy grids and a prototype 
forward looking sonar will be considered. Providing the vehicle with three-dimensional 
views of the environment will allow for optimal route planning and an increase in 


successful missions in complex environments. 
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EXECUTIVE SUMMARY 


Unmanned underwater vehicles (UUV) are being tasked with a growing number 
of mission sets. These mission sets require the vehicle to perform tasks in a wide variety 
of environments. The mission sets may require the UUV to traverse up river ways, 
perform searches in harbor environments, or navigate in littoral areas. In each of these 
areas, the complexity of the environment poses challenges for the safe navigation of the 


vehicle. 


The current build of UUVs are utilizing sensors that provide two-dimensional 
representations of their operating environment. In compressing the information from a 
three-dimensional world into a two-dimensional representations a significant amount of 
information is lost. The loss of this information creates challenges in the proper 
navigation of the vehicles. The main goal of this work is to provide the vehicle with a 
three-dimensional representation of the environment that can be utilized for obstacle 


avoidance and route planning. 


With this goal in mind, forward looking sonar (FLS) was chosen as the sensor of 
interest. This research mounted two prototype FLS onto the Remote Environmental 
Measuring UnitS (REMUS) used by the Center for Autonomous Vehicle Research 
(CAVR) at the Naval Postgraduate School. One of the sensors provided a horizontal 
field of view that covered ninety degrees and ninety meters in front of the vehicle. The 
horizontal FLS also has fifteen degrees of ambiguity in the vertical direction. The other 
FLS was mounted to provide a vertical field of view that covers forty-five degrees and 


ninety meters, with fifteen degrees of ambiguity in the horizontal direction. 


An occupancy grid was used as the framework of the reconstruction. Occupancy 
grids divide the area into sub regions. Each region or cell is assigned a probability that 
indicates the likelihood of that cell being occupied. The probabilities of occupancy are 
determined from physical and probabilistic models of the sensors. Each measurement 


received by the sensors is converted into a probability of occupancy and provides an 


xiii 


update to the overall probability stored in the grid. With enough measurements, a clear 


three-dimensional representation can be reconstructed. 


To prove the applicability of this method, REMUS was deployed in the Charles 
River in Boston, Massachusetts and directed underneath the Massachusetts Avenue 
Bridge. The bridge provided obstacles that have distinct three-dimensional shapes. 
Using this dataset and the models and algorithms developed the main features of the 
bridge, the berm and pylons were reconstructed. The resulting three-dimensional grid 
clearly depicted the height, width, and depth of the pylons. The grid was also able to 
locate the rapid shoaling of the berm. 


The two-dimensional representations of the bridge depict the bridge as an 
unnavigable area. By successfully locating these obstacles and determining the open 
space surrounding them, the vehicle can safely navigate underneath the bridge. Three- 
dimensional reconstruction of the environment extends the areas in which the vehicle can 


safely be deployed. 
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I. INTRODUCTION 


A. MOTIVATION 


During the last several years there has been an explosion of interest in unmanned 
underwater vehicles (UUV). UUVs are being utilized to explore the ocean depths, survey 
and aid in salvage operations, and are even used in keeping our soldiers and sailors safe. 
The current crop of UUVs is utilized in areas where the main navigational hazard is the 
ocean bed. The next generation of UUVs will need to be able to navigate in water with a 


variety of obstruction: pier pylons, kelp beds, oil platforms, etc. 


B. STATEMENT OF PROBLEM 


UUVs are currently limited by the navigational sensors they employ. The sensors 
primarily map the environment into a two-dimensional representation. While this works 
well for unmanned ground vehicles mapping a hallway, it leads to many complications 
when traversing in the undersea environment. In the past UUVs have been employed in 
locations with a large open area. They have successfully mapped the bottom contours 
and other bathymetric features. However, the next generation of UUVs will need the 
ability to navigate among a wider variety of obstacles. For example, a UUV may be 
required to conduct searches in a congested harbor area and do so several times a year to 
measure and detect changes in the environment. This adds a new level of navigational 


complexity. 


In order to safely navigate in an unknown environment the vehicle must be able to 
image the space directly in front of it. Forward Looking Sonar (FLS) can provide 
submerged vehicles with this capability. Current FLS map the area into two-dimensional 
images. During the process of compressing a three-dimensional world into a two- 
dimensional image information is lost. In some circumstances, that information can be 


critical for proper navigation. 


Figure | is a picture of the Massachusetts Avenue Bridge in Boston MA. Figure 2 


is a rough approximation of the undersea support for the bridge. There are several distinct 


features. Each pylon has an associated width, height and depth. The river bottom quickly 
shoals on each side of the bridge. The shoaling on either side is to support the pylons. 





Figure 1. Massachusetts Avenue Bridge in Boston MA, 
from [18] 


Bridge Pylons 





Figure 2. Submerged Bridge Pylons 


) 


Currently, UUVs are using FLS for obstacle avoidance and path planning ([1] 
[2]). The NPS REMUS vehicle is currently equipped with both a vertical and horizontal 
FLS. Figures 3 and 4 show both horizontal and vertical FLS sonar images from a UUV 
passing underneath the bridge. 


a Rapid Shoaling 





Figure 3. Horizontal FLS Image from a REMUS vehicle navigating underneath the 
Massachusetts Avenue Bridge 


River Surface 
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Figure 4. Vertical FLS Image from a REMUS vehicle navigating underneath the 
Massachusetts Avenue Bridge 
3 


The first feature of note is the quickly shoaling river bottom. According to the 
horizontal FLS, the change in depth of the river bottom appears as a solid wall. 
However, data provided by the vertical FLS shows that the vehicle can simply change its 
depth and safely navigate above the river bottom. The information from the horizontal 
image would prevent the vehicle from approaching the bridge. The pylons of the bridge 
present a similar problem. They are clearly visible in the horizontal image, but again 
show as a solid object in the vertical image. This registers with the obstacle avoidance 


algorithm as a rigid boundary. 


The main point is that when relying either on the individual vertical and 
horizontal images the vehicle would not be able to find a safe path for navigation under 
the bridge. The bridge effectively becomes a blockade that prevents further navigation. 
This is a severe limitation for UUVs. In order to overcome this, a method must be 
developed to take the two separate sources of information and merge them into a more 
coherent single three-dimensional environmental model. A model that allows the vehicle 


to locate the opening between the pylons. 


UUVs are being called upon to accomplish increasingly complex tasks, such as 
navigating up rivers. This requires detailed knowledge of the three-dimensional world in 
which the UUV is located. In order to accomplish tasks such as these, the vehicle will 


need the benefits provided by a three-dimensional model of the area. 


C: SCOPE AND STRUCTURE OF THESIS 


This thesis will look into the feasibility of reconstructing a three-dimensional 
environment through the use of two-dimensional horizontal and vertical forward looking 
sonar images. The creation of the three-dimensional environment is considered critical 
for the UUV to navigate in cluttered and restricted waterways. In order to create a three- 
dimensional model, a probabilistic model of the forward-looking sonar will be developed. 
There are two aspects to the model — the vehicle motion model and the sensor model. The 
sensor information is only as good as the estimate of the vehicle’s position. The thesis 
will start with a description of the vehicle and a probabilistic model for estimating 


position and errors. 


Next, the sensor model will be defined. The model will define the spatial 
relationship described by the sonar images. This relationship will be used to determine 
an object’s location in global environment. This FLS model will also need to define both 


the probability of detection and the probability of noise attributed to each sensor. 


Following the FLS model description, an occupancy grid will be developed. This 
three-dimensional grid will be the representation of the environment. The probabilistic 
model of the vehicle’s position will be combined with the probabilistic and spatial 
models of the FLS to populate the occupancy grid. The following chapters will discuss 
the generation of the FLS model, the basis for occupancy grids, and will culminate with 


the algorithm used to create the three-dimensional model. 


D. PRIOR WORK 


Three-dimensional mapping has been considered for many applications. Robotic 
vehicles have employed various techniques to determine their environment, whether they 
are entering damaged buildings for disaster relief [3] or mapping the ocean floor [4]. 
Unmanned vehicles have also been employed with a wide range of sensors. The sensors 
utilized in three-dimensional mapping include cameras, laser range finders, radar, and 


multiple forms of sonar. 


The current methods used in mapping the undersea environment are focused on 
producing maps after the vehicle has transited through the area. These methods typically 
involve a side scan sonar. Side scan sonar uses a specific array of active sonar heads to 
provide imagery of the ocean floor to the side of an UUV. Side Scan sonar can produce 
very accurate three-dimensional measurements, but will not provide images of the 
volume directly in front of the vehicle. With the goal of obstacle avoidance, the use of 
side scan sonar provides little benefits. Forward-Looking Sonars have previously been 
used for reactive obstacle avoidance with success ([1]& [2]). Nevertheless, in this past 
research the vehicle was limited to a singular plane of view, either vertical or horizontal. 
Little research has been conducted on combining multiple FLS into a single 
representation. The thesis will look into the feasibility of utilizing FLS to develop a 


three-dimensional map. 
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I. VEHICLE DESCRIPTION 


A. VEHICLE DESCRIPTION 


The Remote Environmental Measuring UnitS (REMUS) is the vehicle used in this 
thesis. It is a multipurpose unmanned underwater vehicle (UUV) currently in use by the 
Center for Autonomous Vehicle Research (CAVR) at the Naval Postgraduate School 
(NPS). REMUS is a small man-portable UUV ([5]). REMUS specifications are listed in 
Table 1. This small package provides for easy deployment and recovery. Coupling its 
ease of deployment with a reconfigurable payload allows REMUS to be capable of 


executing a wide assortment of missions. 


Maximum Diameter 19cm 
Maximum Length 160 cm 
Weight in Air 37 kg 
Trim Weight lkg 
Maximum Operating 100m 
Depth 


Energy lkw-hr Lithium Ion Battery 
Endurance 22 hours at 3 kts 
>8 hours at 5 kts 


Propulsion DC brushless motor, 3 _ bladed 
propeller 
Velocity Up to 5 kts 


Table 1. © REMUS Specifications, from [5] 











Figure 5. | NPS REMUS onboard The Cypress Sea 


REMUS can be equipped with various environmental sensors including 
temperature and conductivity sensors, pressure sensors, fluorometer, and turbidity 
sensors. In addition to the various environmental sensors, it is also capable of carrying a 
wide range of imaging sensors, to include side scan sonar, forward looking sonar (FLS), 
and video cameras. Navigationally, the vehicle may choose from many options. The 
navigational sensors include GPS, inertial navigational system (INU), long or ultra short 
baseline, and Acoustic Doppler Current Profiler (ADCP). This wide range of payload 
options allows REMUS to be used in operations that range from scientific surveys to 


harbor security. 


The REMUS vehicle employed by the CAVR is a modified version. The CAVR 
REMUS is equipped with a prototype FLS provided by BlueView Technologies. The 
prototype FLS will be the main sensor utilized in this thesis. This FLS will be discussed 
at length in the following chapters. 


B. EQUATIONS OF MOTION 


To take full advantage of the sensors employed, the position of the vehicle is 
required. Like all vehicles, REMUS position at any given time is a result of a complex 
set of forces. A single paper could be dedicated to the description of the forces affecting 
underwater vehicles. The purpose of this section is to provide the reader with a 
simplified overview of these forces and how they are modeled. A more detailed 


explanation can be found in [7]. 


To discuss the forces acting upon a body, a common reference frame must be 
defined. The standard approach to relate the position and motion in two separate frames 
of reference is the Newton Euler approach. The coordinate systems used in this approach 


can be seen in Figure 6. 


Global Frame 






Local Frame 


Figure 6. Local and Global Coordinate System, from [7] 


The following assumptions are made for this model: 
" The rotation of the earth does not appreciably contribute to the 
acceleration of the vehicles center of mass. Therefore, the rotation of the 


earth is negligible. 


" The vehicle is modeled as a rigid body 


« The sources of significant forces acting upon the vehicle are propulsion, 
hydrostatic, and hydrodynamic lift and drag. The forces can all be 


classified as either inertial or gravitational. 


The local velocity is a vector composed of surge (u), sway (v), and heave (w). 


we Pane aes ; 
The conversion from the global velocity [ hg the local velocity requires a 


transformation matrix. 


x xX 
ya dalle 2-1 
Z Z 


T, the transformation matrix, is defined in terms of ‘Euler’ angles (@,0,y) and is 


listed in Equation 2-2. 


cosy cos 0 siny cos@ —sind 
T (¢,0,v) =| cosysind@sing—sinycosg sinysin@sing+cosycos¢ cosOsing 2-2 


cosysin@cosé+sinysing sinwsin@Ocosé@—coswsing cosOcos¢ 


Likewise the rate of change of the ‘Euler’ angles can be defined in terms of the 


global angular velocity vector [p,q,r]. 


p 1 singtanO cos¢tané@ || p 
6 |=|0 cos¢ —sing q 2-3 
y 


‘ si Ts 0 a Vs 7 : 


From these equations, Healey derived six equations of motion for a rigid body. 
The equations of motion defined as Surge (Equation 2-4), Sway (Equation 2-5), Heave 
(Equation 2-6), Roll (Equation 2-7), Pitch (Equation 2-8), and Yaw (Equation 2-9). 


Table 2 lists the variables and definitions used in the equations of motions. 
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,=m|u, vr+wgq ee r) eae (W —B)sin@ 


x 
Y,=m ml, +u,r—w,q+Xo(patt)—yo(pe+r —(W —B)cosOsing 








Z, =m| Ww, u,gtv,p+XxXq(prt+q)t+yo(qr+4) pe ae (W — B)cos@cosO 
K, ee ee nea eee -1°)-1..(pqt?) 
+m| yg (w-4,9 +, P)—% (0, +u,r—w,p) | 
—(9,W — y,B)cos@cos¢+(z,W - z,B)cos Osing 
M, =1,4+(I,-1,) pr-1, (qr-p)+1,,(pq-?)+1,,(p’ +r’) 
-m| Xx, (w-u,qtv,p)— 2, (u, —v,r+w,Q) | 
+(x,W +x,B)cos@cos¢ +(z,W -z,B)sing 
N, =? +(1,-1,)pq-L,(P’ —q°)-1,.(pr+q)+1,.(qr-b) 
+m| x, (v, +u,r—w,p)— Yo (a, —v,r+w,q) | 
—(x,W — x,B)cosOsing—(y,W — y,B)sin0 





























Ix, I), 1, _| Mass moment of inertia terms 
Component velocities for a rigid body fixed 
Ur Vr, Wr 
system with respect to the water 
F Component angular velocities for a rigid body 
Bb fixed system 
Positional difference between center of 
aa buoyancy and the geometric center 
Positional difference between the center of 
ae eG gravity and the geometric center 
B Buoyancy 
W Weight 
6,(t) Delta Rudder function 











Table 2. | Equation of Motion Variables 


Dolbec [8] coupled the equations of motion with the hydrodynamic coefficients 


associated with REMUS [9] to create the final kinematics equation used to model the 


motion of REMUS (Equation 2-10). 
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C: POSITION ESTIMATION 


The equations of motion provide the theoretical framework for determining a 
vehicle’s position based upon the forces acting upon it. For the equations of motion to 
provide any practical benefit they must be coupled with a sensor capable of measuring 
the acceleration and angular rates in each of the component directions. For a UUV the 


sensors include the following: 


1. Acoustic Doppler Current Profiler (ADCP)/Doppler Velocity Log (DVL) 


— It provides: 
a. Water velocities — surge, sway and heave 
b. Vehicle velocities over the ground — forward, side slip and heave 


c. Vehicle Altitude 


2, Depth sensor 

3. Compass — Heading and heading rate 

4. Global Positioning System — Latitude and Longitude position on the 
surface 

>. Inertial Navigation Unit (INU) — Accurate angular and motion velocities 


These sensors are combined together using an Extended Kalman Filter (EKF) to 
provide an optimal position estimation of the vehicle. |The theory of the EKF, a 
derivative of the Kalman filter, has been discussed in depth in many publications; this 


chapter will serve simply as an overview of the EKF and its application to REMUS. 


In a Kalman filter, the state and variance of a dynamic linear system is determined 
recursively. The Kalman filter is a two-step process. The first step is to predict the state 
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and variance of the system using the model of the system (movement update). For 
REMUS the equations of motion serve as this model. Upon receiving a new 
measurement, the difference between the measurement and the predicted state is 
calculated. This difference is a correction factor used to update the current state of the 


system (measurement update). The process is depicted in Table 3. 





Movement His = PU; + U,, 
eer Xi =P P'+O 
Measurement _ 2H! 

Gt geet, eae 
Update HY,,,H'+R 


Hisiz =, Hist +K(Z — Hh, -Hy) 
Dae UE KE ID 


i+]|Z 





Variable 


ns 


#2): mean and covariance of the system state 


Definition 


~~ 


HM, ,R): mean and covariance of the measurement noise 


aia 


L,,,Q): mean and covariance of the movement noise 


H: measurement matrix 
K: Kalman Gain 

Z: measurement 

g: movement matrix 











Table 3. Kalman Filter Equations, after [10] 


The method listed in Table 3 will only work for systems with a linear model. 
However, modeling the position of REMUS is a non-linear process. Since REMUS does 
not measure position directly, but deals with the estimation of position from both angular 
rates and accelerations the equations of motion are non-linear in nature. Because of the 
non-linearity an EKF is required. In an EKF, the nonlinear components are linearized by 
an approximation function (normally a Taylor series approximation). In an EKF the 


following substitutions are made: 
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= (u, x) is replaced with f(u, X) 
* Zis replaced with h(Z) 


The most accurate sensor for measuring the global position of the REMUS was GPS. 
This is only available when the vehicle is on the surface. Once underwater, position 
estimation errors grow more rapidly over time. For the UUV to navigate accurately for 
extended periods of time underwater, it requires a high-grade inertial navigation system. 
The REMUS UUV had an integrated navigation suite that combines together the IMU, 
GPS and ADCP/DVL. It is the SEA DeViL T24, a navigation suite from Kearfott that 
advertises a position accuracy of 0.5% total distance traveled. The stated manufacture 
specifications for REMUS’ IMU are listed in Table 4. Alameda provides a detailed 
explanation of the Sea DeViL architecture [11]. 




















| SEA DeViLl PERFORMANCE* 

| Surface Ship KN-6051 | KN-6052 | KN-6053 
Position Accuracy 
e  GPS/DVL* 10 m, CEP 10 m, CEP 10 m, CEP 
e DVL (Water-Track Mode)**** 10 nm/8hrs, TRMS 2 nm/8hrs, TRMS 1 nm/8hrs, TRMS 
Heading Accuracy 
. GPS/DVL™ 5.0 mils, rms <1.5 mils, rms <1.0 mils, rms 
e = §=DVL 5 mils* secant A, rms 1.0 mils* secant 2, rms 0.5 mils * secant A, rms 
Velocity Accuracy 
e GPS/DVL™ 0.05 m/sec 0.05 m/sec 0.05 m/sec 
e DVL (Water-Track Mode) 0.5 m/sec, rms 0.35 m/sec, ms 0.3 m/sec, rms 
Roll/Pitch Accuracy 0.5 mils, rms 0.5 mils, rms 0.5 mils, rms 

Underwater Vehicle*** KN-6051 KN-6052 KN-6053 

Position Accuracy **** 0.5% DT, CEPR 0.2% DT, CEPR 0.05% DT, CEPR 
Heading Accuracy 5 mils*, rms 1.5 mils*, rms 1.0 mils *, rms 
Roll/Pitch Accuracy 0.5 mils, rms 0.5 mils, rms 0.5 mils, rms 




















Table 4. | Sea DeViL Performance From Kearfott Systems, from [12] 


During this experiment, the vehicle did not receive any position measurements 
after the initial dive. The position and the errors associated with it will be entirely 
attributed to the accuracy of the Sea DeViL. Due to the short relative distance traveled 
during experiment, the errors in position are minimal. The vehicle traveled 


approximately 450 meters to reach the bridge. This equates to a possible positional error 
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of 2.3 meters. The total distance traveled at the beginning of the last pass underneath the 
bridge was 1350 meters, yielding a maximum expected error of 6.5 meters. For this 


reason, the position and orientation will be assumed to be relatively accurate. 


The position of the vehicle (or mobile sensor) and its orientation play a critical 
role in mapping the output of the sensor to the global space. All of the information 
provided by the sensor is referenced to the vehicles location and attitude. Any error in 
the vehicles location is propagated through each of the models. In other words, errors in 
the vehicle’s position or orientation will cause objects to be plotted in the wrong location, 


invalidating the entire method. 


The next chapter will discuss the model of the FLS. The FLS model will then be 
combined together with the vehicle model in order to take the sonar images and 


reconstruct a three-dimensional occupancy grid that represents underwater objects. 
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HI. FORWARD LOOKING SONAR 


A. INTRODUCTION 


A sonar uses the distribution and reflection of a sound wave to determine the 
range and bearing to submerged objects. Active sonar systems use a transducer to create 
their own source of sound in the water (called a ping) and then listen for its return. In its 
most basic form, active sonar determines the distance to a given target by measuring the 
time delay from the production of a sound until the return reaches the transducer. In 
order to determine the distance to a target, all that is required is the total travel time and 


the speed of sound through the water. This yields Equation 3-1. 


__ time delay * Speed of sound 


Range 
is 2 


3-1 





The accuracy of the range is then dependent upon the ability of the system to 
accurately measure the time delay and the accuracy of the speed of sound. Ideally, the 
speed of sound would be constant throughout the water, however, in practice this is not 
the case. The approximate speed of sound is calculated onboard the REMUS vehicle 


using water temperature and salinity. Normally, this is about 1500 meters per second. 


Ranging information alone is not sufficient to geo-locate an object. To accurately 
determine the location of the object it’s bearing must also be determined. To measure 
the bearing to an object, a transducer will only project or listen to sounds along a 
particular bearing (also called a beam). Thus by only listening to sounds along a 
particular bearing and measuring the time delay of the sounds return, both the bearing 
and the range to an object can be determined. The ability of the transducer to listen along 
a particular bearing or range of bearings is called its directivity. A transducer with a high 
directivity will be able to isolate sounds to a small range of bearings. Transducers with 


low directivity have larger bearing resolutions. 


For the sonar to determine the range and the bearing to an object it must be able to 
distinguish the return from the background noise. There are numerous sources of noise 
for any sonar system. In the underwater environment, multiple sources of sound exist. 
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Everything from biologics to merchant traffic emits sounds across all frequencies. Wave 
action and rain also contribute significantly to the amount of background noise a sonar 
system must overcome. The overall ability of the sonar system to determine detection is 
based on the ratio of return signal received at the transducer to the amount of general 


noise received at the transducer. This is the sonar’s signal to noise ratio (SNR). 


Due to the larger range of values of sound energy in the water, SNR is generally 
defined in terms of decibels (dB). The general equation for SNR in an active sonar 
system is shown in Equation 3-2. 

SNR = SL-2TL+TS — NL 3-2 

The amount of signal returned to the transducer is dependent upon the amount of 
sound initially projected into the water, the source level (SL). After leaving the 
transducer, the sound expands and travels through the medium towards the object. While 
traveling, the intensity of the sound is diminished via attenuation and expansion. This 
loss of intensity is called the transmission loss (TL). The total amount of sound received 


at the object is the source level minus the transmission loss. 


Once the sound reaches an object in the water, it is then reflected off the object. 
The amount of sound reflected and directed back towards the original source of the sound 
is called the target strength (TS). Many factors affect the amount of sound reflected off 
the object. TS is dependent upon the angle of incidence, the absorption properties of the 
object, the size and shape of the object, etc. Once the sound is reflected, it travels back 
towards the source. As it travels, it undergoes the same amount of transmission loss. So 


the total pressure received at the transducer from original signal is 


Received Signal = SL - 2 TL+TS 3-3 


However, this does not take into account the amount of noise the transducer 
receives. Letting the noise level (NL) be defined as the amount of noise received by the 
transducer, the SNR equation becomes 


SNR = SL—2TL+TS — NL 3-4 
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B. BLAZED SONAR ARRAYS 


Blazed array sonar is a method to create a highly directive signal in a small 
package. Blazed array sonar systems utilize a principle similar to echelette diffraction 
gratings in optics to create a multitude of beams from a single sound source [13]. The 
blazed sonar array maps the frequency of a given broadband pulse to the angular spatial 
domain. The principle is similar to that of a prism. A broad spectrum signal (white light) 
is processed through the stave (the prism) and individual frequencies are projected to 
independent spatial locations (the rainbow). Each frequency band creates a unique beam. 
The shape and size of the beam is dependent on the frequency band and the shape of the 
stave projecting the sound. A plot of the magnitude of the projected sound versus bearing 
for a typical blazed array sonar is provided in Figure 7. The lower larger beams were 


created by the lower frequencies. 
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Figure 7. Composite Blazed array beam patterns for frequencies between 300 kHz 
and 600 kHz, from ([15]) 
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Once the sound has been transmitted into the water from the blazed array, it 
interacts with its environment just like any other active sonar. The main difference is that 
the frequencies are separated into individual beams. As all the frequencies return to the 
transducer, they are recombined into a single signal. The recombination of the signals 
occurs in reverse order of the creation. Each beam will only receive a particular band of 


frequencies. This maintains the original frequency to angular spatial mapping. 


Since each frequency band correlates to a specific angle in the spatial domain, the 
individual frequencies can be modeled as a single hydrophone with a high directivity. 
Maintaining the frequency to angular spatial relationship creates a simulated array of 
hydrophones. This simulated array is what creates the detailed representation of the 


volume ensonified. 


C. BLUEVIEW P450-15E 


The BlueView P450-15E is a blazed array system with a center frequency of 450 
kHz. The P450-15E specifications are listed in Table 5. 


P450E-15 Sonar 


Power | 12-48 volts @ 10 watts (with 50ft cable) 





Operating Frequency 450kHz 


Table 5. | BlueView P450-15E Specifications (from BlueView Inc., 2008) 
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The P450-15E produces image files in both Cartesian and Polar coordinates (see 
Figure 8). The resolutions available according to image type are listed in Table 6. 
While the P450-15E is capable of higher range resolutions, it is constrained by the size of 
the image available. Due to the nature of the blazed array sonar, the polar coordinate 
system is the natural coordinate system. In order to transfer the information from the 
polar coordinate system to the Cartesian image, pixels with changing resolutions were 
required. In the polar image the resolution per pixel is constant throughout the entire 


image. For these reasons, polar images where utilized in this thesis. 


Cartesian Image Polar Image 





























Figure 8. Cartesian and Polar images from P450-15E sonar 
BlueView Cartesian Polar 
Specification 
Image Size (in 387 x 571 461 x 2048 
pixels) 
Range 2 inches Appoximately 90m 767% 
Resolution 12” x 6” 461 pixels rive 
Bearing 1 deg varies based on 90° gant 
Resolution the location of) 2048 pixels ~ Tre 
the pixel 








Table 6. | Comparison of Horizontal FLS Image Resolutions 
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Both image files provide an angular resolution greater than the manufacture 
specifications. This anomaly is attributed to the varying beamwidths of the blazed array 
sonar. The larger bearing resolution listed by the manufacture corresponds to the larger 
beamwidth of the lower frequency beam. However, the resolution of the polar image 


appears to be based upon the beamwidth of the highest frequency beam. 


The image created by a P450-15E is a two-dimensional representation of a three- 
dimensional volume. A depiction of the actual volume ensonified is depicted in Figure 9. 
A single stave of the P450-15E has an approximate 23-degree field of view in the image 
plane. The NPS REMUS AUV has four staves in the horizontal plane for an approximate 
90-degree total field of view. 





Figure 9. Volume ensonified by the horizontal P450-15E 


The ensonified volume covers a fifteen degree spread in the vertical plane. The 
sonar is unable to distinguish the difference between an object at different elevations. 
For example the object highlighted in Figure 10, could be located anywhere within the 
fifteen-degree spread. This is the ambiguity of the sonar in the vertical plane. The object 
shown in Figure 10 is approximately thirty meters in front of the vehicle. Based on its 
range and the bearing ambiguity of the FLS, the object can be located within +/- 4 meters 
in altitude. The P450-15E provides a range resolution of 2 inches and a bearing 
resolution in the image plane of 1 degree. This results in the ensonified volume being 
divided into cube like shapes with dimensions of 2 inches by | degree by 15 degrees (see 


Figure 11). 
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Figure 10. | Submerged Rock in a Horizontal FLS Image 





Figure 11. | Volume segments created by P450-15E 


The vertical array is composed of a single stave mounted perpendicular to the 
horizontal array. Mounting the array in this fashion provides angular resolution in the 
vertical direction. Due to the rotation of the array, the ambiguity of the sonar shifts to the 
horizontal direction. All vertical sonar images will have fifteen degrees of ambiguity in 
the horizontal direction. The vertical sonar will be utilized to provide the location of the 
ocean floor and the height of the objects encountered. A typical vertical sonar image is 
displayed in Figure 12. 
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River Surface 


River Floor 





Figure 12. Vertical Sonar Image depicting the river surface and floor 


By combining the separate fields of view, the two sonars can effectively create a 
volume with resolution in all three planes. The volume ensonified by the combination of 
sonars is depicted in Figure 13. Of particular importance is the fifteen-degree by fifteen- 
degree area where the two arrays overlap. This overlapping of the arrays divides that 
volume of water into sections that are two inches in depth (vertically and horizontally) by 


one degree in vertical width and one degree in horizontal width (Figure 14). 
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Figure 13. | Volume ensonified by the combination of Vertical and Horizontal Arrays 


1 deg 





Figure 14. Volume segments created by the overlapping sonar 


The vertical and horizontal sonar are independent sonar systems. Information is 
not shared between them. Separate two-dimensional representations are created by each 
system. The horizontal system provides resolution in the horizontal plane, but is unable 
to provide resolution in the vertical plane. The vertical system, though unable to provide 
resolution in the horizontal, provides resolution in the vertical. In order to create a three- 
dimensional model the two separate data streams need to be combined. The following 
chapters will discuss the use of occupancy grids to combine the separate systems into a 


single three-dimensional model. 
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D. FLS PROBABILITY MODELS 


Knowing that the purpose of the FLS sonar is to populate an occupancy grid 
necessitates the development of probabilistic models of the FLS. For a sensor to be used 
in an occupancy grid, two separate probabilistic models need to be generated. One model 
describes the probability of receiving a value when the return can be attributed solely to 
noise. The second model describes the probability of receiving a particular value when 
the sonar return can be attributed to a physical object. This section will focus on the 
development of the probability models and the details of an occupancy grid will be 


discussed in the next chapter. 


1. Horizontal FLS Noise Model 


There are many possible sources of noise for the sonar system. Like all electrical 
systems, a certain amount of noise can be attributed to the various types of electrical 
noise: power supply fluctuation, thermal noise, etc. A secondary source of noise is 
acoustic. Acoustic noise includes weather, biological sources, or other vehicles in the 
area. Acoustic Models have been created for the various types of noise, but these models 
do not take into account the method of beamforming used in blazed array sonar. The 
final category of noise can be attributed to REMUS itself. REMUS utilizes several other 
sensors that also project noise into the water column. These sensors include but are not 


limited to the acoustic modem, ADCP and Side Scan sonar. 


Instead of designing and incorporating theoretical models for each of the noise 
sources an experimental approach was taken to create a comprehensive noise model. 
Within the dataset used, there are approximately 300 images for the horizontal FLS that 
show little to no objects in the field of view. These images were acquired with the 
vehicle in motion and in the same acoustic environment. By using images taken in the 


same acoustic environment, the noise can be classified as a single distribution. 


The expected distribution of the noise will be a combination of multiple noise 
sources. Because the resulting distribution of the noise is unknown, a histogram was 


used to observe the distribution. By using a histogram, an initial assumption of the 
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distribution does not need to be made. When creating a histogram, the bins size and 


spacing can play an important role is determining the distribution. 


The number of bins will be determined by the range of values expected. Each 
image in the data set is a sixteen bit image, so each pixel has a potential range of value 
from zero to 65535. However, prior experience has shown that the maximum expected 
value of noise is much lower. In determining the bin size and spacing a subset of 50 
images were analyzed. The combined histogram of each of the images was plotted. In 
these fifty images, the maximum value of noise was 3958. Based on the maximum noise 
value and considering the computer storage limitations the noise histogram contained 25 
bins with a spacing of 160 (with the exception of the first and last bin). The first bin 


contained values from zero to 80; the last bin contained values from 3920 to 65536. 


With the bin size determined, the images were analyzed in more detail. Due to 
the nature of the FLS, the noise distribution varied based on the range and bearing of the 
pixel. As shown in Figure 15 the distribution cannot be readily classified. The 
distribution of noise in the larger ranges appears to be a Gaussian distribution, where the 
distribution at close range is a combination of distributions. This more complex 
probability distribution function may be attributed to the near field effect of active sonar. 
To take into account the variability of the distributions across the entire image, a 


distribution was defined for each pixel in the image. 
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2: Horizontal FLS Detection Probabilities 
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Noise Distribution Changes over Range (meters) and Bearing (degrees) 


The other distribution required for the occupancy grid is the probability of 


receiving a particular value given that the value is attributed to a return from an object. 


Ideall, this model would be based on a theoretical approach. Theoretical models exist for 


determining sound propagation in the water. Using the models, the intensity level of 


sound can be predicted at any point in the water column. 


However, the algorithms 


utilized by BlueView in mapping the broadband signal received at the FLS stave into an 
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image are proprietary. Without knowledge of the exact algorithms involved in generating 


the FLS images, an experimental approximation must be made. 


A single experiment has not been conducted to determine the distribution of 
expected received values at every pixel in the image. To determine the distribution of 
received FLS image values when an object is present a subset of the Charles River 
dataset was analyzed. Unfortunately, every image in the Charles River dataset is subject 
to all the noise sources previously discussed. The noise distribution must be separated 


from the signal distribution. 


The subset of images used for this portion of the experiment was chosen due to 
the large area that the objects filled in the field of view. In a similar fashion to the noise 
distribution, the histogram of each pixel was determined. The following process was 
used to separate the detection distribution from the noise distribution, and is illustrated in 


Figure 16. This procedure is done for each pixel in the image. 






























































a. Noise Distribution b. Normalized Noise Distribution, n 
100 1 
50 | 0.5 
0 : : 0 
-100 0 100 200 300 0 100 200 
c. Detection Distribution d. Normalized Detection Distribution, s 
100 1 
50 | 0.5 
-100 0 100 200 300 0 100 200 
e. s-n f. (S-n) raised to a min of zero 
1 1 
0.5 0.5 
0 0 
0 100 200 0 100 200 
g. Adjusted Detection Distrobution h. Adjusted Detection + Noise 
1 
0.5 0.5 
0 0 
0 100 200 0 100 200 


Figure 16. | Determining the Probability of Detection for pixel (50, 400) of the 
Horizontal FLS 
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1. Determine P[pixel value | no Object] (Figure 16a). This is a histogram of 


the raw data retrieved from the dataset. 
2. Normalize P[pixel value | no Object] (Figure 16b). In this step the 


histogram is normalized so that the sum of all of the bins equal one. 
3. Determine P[pixel value] (Figure 16c). The raw histogram of the dataset 


that contains images with a combination of objects. 
4. Determine P [pixel value | Obj ect | . The probability of receiving any pixel 


value is a summation of multiple probabilities (Equation 3-5). 
P[pixel value] = P[pixel value | Object | P[ Object] + P[pixel value | no Object] P[no Object] 3-5 


Rearranging Equation 3-5 yields Equation 3-6. 


P[Object | _ 


l 3-6 
P[no Object] P[no Object] 





P[pixel value | Object] P[pixel value] - P[pixel value | no Object] 
P[no Object] was estimated by the ratio of the sum of histogram bins of 
P [pixel values| that have a corresponding non zero bin in 


P[pixel values|no Object]. Figure 16d 


shows Fs open ine values | : 
Plobi 
5. P[pixel qalueobee = es, shown in Figure 16e. 
P[no Object] 
Plobi 
6. Ensure that all values of P[pixel anidObjeaf remain 
P[no Object] 


positive (Figure 16f). If any value in the histogram generated in step five 
is negative, raise all values of the histogram by the maximum negative 
value. This ensures that all probability values are non-negative. 


: . ; ; P [ Object | 
7. Determine P[pixel values | Object ] (Figure 16g). Assume ————— 
P[no Object] 
P| Object 
is a constant and normalize P| pixel eahucoujee ee This 
P[no Object] 


provides P[pixel values | Object], the final distribution used to model the 


probabilities of receiving a value given that an object is present. 
8. Ensure P[pixel values |no Object]+ P|pixel values | Object] maintains the 


relationship of the original distribution (Figure 16h). To verify that the 
separate distributions are accurate, they are summed and normalized. The 
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new normalized combination distribution is compared to the original 
normalized distribution. As can be seen in Figure 16d and h, the resulting 
shape of the distribution is similar to the original. 


3. Vertical FLS Model 


The Vertical FLS is comprised of staves identical to those used in the horizontal 
FLS. The difference between the vertical and horizontal FLS is the number of staves 
used and the orientation in which they are mounted. The vertical FLS covers 45 degrees 
in the field of view (2 staves) and the horizontal FLS covers 90 degrees (4 staves). The 
central 45 degrees of the horizontal sensor distributions should be identical to the 


distribution of the vertical FLS. 


4. Verification of the Probability Models 


The models are created on a pixel-by-pixel basis; therefore the horizontal model is 
comprised of almost two million individual probability functions (461x2048x2). With 
that number of distributions it was impractical to inspect each distribution. To verify that 
the relationship between the noise and detection distributions were valid, a sample 


Bayesian probability image was created for each sensor. 


P(J| all pixels are due to detections 
Bayesian Image = ( MED 3-7 





P(1| all pixels are due to detections) + P(J| all pixels are due to noise) 


If the distributions are correct then the Bayesian image will show a high intensity 
value for any obstacle in the image and will show a very low intensity in areas due to 
noise. Figures 17 and 18 show the original FLS image with the resulting Bayesian 
image. As can be seen in both figures, the amount of noise is drastically reduced and the 
objects detected are greatly enhanced. The clarity of the images proves the validity of 
both the noise and detection distributions. As can be seen Figure 17, further future work 


can be done to remove the noise in the lower portions of the image. 
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Figure 17. Horizontal FLS Image (upper) and corresponding Bayesian Image (lower) 











Figure 18. Vertical FLS Image (left) and corresponding Bayesian Image (right) 
Both images are in polar coordinates 
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E. GEO-LOCATING IMAGE DATA 


Up to this point, all of the information provided by the FLS has been referenced to 
the vehicle. To be truly useful in a mapping algorithm, the FLS data will need to be 
mapped into the global space. By combining the vehicle position estimation model with 
the FLS model, each data point in a sonar image can be geo-located. This section will 
cover the equations necessary to couple the models and convert sonar images into to map 


coordinates. 


1. Development of the Transformation Equations 


The height of the image corresponds to a range in front of the vehicle (0 to 90m) 
and the width of the image corresponds to the bearing from the vehicle. The horizontal 
images are 461 x 2048 pixels. The vertical images are 461 x 1024 pixels. Both of these 
are using polar coordinates In the horizontal images, with the four staves, the bearing is 
the spread in the horizontal plane of the vehicle and covers -45° to 45°. The vertical 
images cover from -22° to 22° in the vertical plane of the vehicle (There are two staves 
and the coverage is a total of 45 degrees). Equation 3-8 shows the conversion from pixel 


indexes into the range and bearing from the vehicle. 


Horizontal Image Vertical Image 
r, =i Max Range + =i* Max Rage 
Image Height R Image Height 
0 ey ee 0 =-0 
Image Width ” 
24° 
=0 = —]2° + j*—____ 
ie is p Image Width 3-8 
where 


r,: Range in front of the vehicle 
@, : rotation about the z axis in the XY plane 
y,, : rotation about the x axis in the YZ plane 
( ri ) : Pixel location (height, width) 
To fully describe the volume ensonified, Equation 3-8 needs to be revised to 


include the ambiguity of the FLS. At this point, a decision needs to be made to determine 
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the resolution associated with the ambiguity of the FLS. Since the overall goal of the 
system is to combine both sensors into a single coherent sensor model, the resolution of 
the ambiguity is chosen to match the resolution of the opposing sensor. This provides a 
one-degree spacing in the ambiguity of the sonar images and causes 0, and @, to become 


vector quantities. 





Horizontal Image Vertical Image 
P _ 4 90m 2 _ 4 20m 
: 461 a 461 
90° 3-9 
0, =-45° + j* @, =}-7,-6,...7 
P 54g = 
24° 

= 4-7,-6,...7 =—12° + j*— 

vem pM 1 1024 


Combining the knowledge of the vehicles position and orientation with 
Equation3-9, allows for each pixel in the image space to be geo-located. Define the 
sensor’s location as (Xo, Yo, Zo) With an orientation of (8,, @y, Wy ). Due to the small 
amount of roll associated with the vehicle it will not be accounted for in the 


transformation. 


(x,,,>%,): Vehicle postion in local reference frame 


0: Vehicle Heading (after conversion into the local frame) 


3-10 
g,: Vehicle Pitch 
y,: Vehicle Roll 
The data provided by a single pixel can then be geo-located using Equation 3-11. 
x,=xX,+7, cos(8, nO, )cos(¢, + d,) 
y,=y, +7, sin(9, +0, )cos(¢, +¢,) rer 


Zp =% +7, sin(¢, +¢,) 


Note: ¢,,0,,x,,¥,5Z, are vectors 


Equation 3-11 will map any data stored in the image space into the global space, 
whether the data is the original sonar image or a processed version. This fact will be 


utilized in the next section during the creation of the occupancy grids. Since the 


34 


occupancy grids use a probabilistic approach, each image will be transformed from the 
original image into the probabilistic image. The probabilities assigned to each pixel will 
then be mapped into the occupancy grid. The next chapter will utilize both the sonar and 


vehicle models to generate the occupancy grid. 
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IV. OCCUPANCY GRIDS 


A. MAP CHOICE 


The main goal of providing the UUV with a three-dimensional map is safe and 
accurate navigation. A safe route is one in which the vehicle has a low probability of 
collision. In other words, the vehicle is able to locate and avoid the obstacles in its path. 
The accurate route is one in which the vehicle is able to successfully move from a starting 
position and reach its end position. This thesis will not delve into the intricacies of route 
planning, but route planning defines the type of map required for safe navigation and thus 
must be considered. These goals provide the requirements for the map. First, the vehicle 
needs to know location of obstacles. Second, the vehicle needs to know the location of 


free space. 


The most intuitive method to create the map is feature recognition. In feature 
recognition every object detected by the sensor is classified. The object’s shape, 
dimensions and location are then stored in a database. Once a new sensor reading is 
received, it is correlated to all the objects currently stored in the database. Previously 


detected objects are verified and the new objects are added into the database. 


Feature recognition is easily grasped conceptually, but can be complicated to 
implement. In man-made areas, such as buildings, the range of shapes required to 
accurately depict the environment is limited, most areas can be approximated by simple 
shapes. In more natural settings, the variation in objects requires more complex shapes 
be defined. The larger dataset of shapes added to the algorithm and the increased 
complexity of the shapes requires large amount of computations. This in turn requires a 


very large amount of processing power. 


With feature recognition, every shape must be classified, compared against the 
database of shapes, and stored for latter comparison. Cluttered environments will require 


a significant number of classifications and comparisons. As the vehicle finds more 
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objects, more processing power and time will be required. The variability in processing 
power requirements makes feature recognition difficult to implement on small scale 


UUVs. 


Feature recognition also provides much finer detail than is required by the UUV 
for safe navigation. For safe navigation, the vehicle only needs to know that an object 
exists as a given position. It does not matter whether that object is cubic, cylindrical, or 
spherical. It only matters that the space is occupied. For that reason, occupancy grid 


mapping is a better choice. 


B. OCCUPANCY GRIDS 


Instead of recognizing and organizing objects by their shape and location, 
occupancy grids divide the area into a grid. The region to be mapped is defined and then 
tessellated. Each element, or cell, in the grid is assigned a value indicating whether the 
cell is occupied. This idea is easily extended from two-dimensional (checkerboard) to 
three-dimensions (stacks of cubes). Since the state of the cell could only take on one of 
two values. The cell is either empty or occupied. Since the cell can only be in one of two 
states, the probability of the cell being empty plus the probability of the cell being 


occupied must equal one (Equation 4-1). 


P[ s(C) = Empty |+ P| s(C) = Occupied |=1 4-] 


The information provided by the sensors does not directly map to the state of the 
cell. The state of the cell must be inferred from the sensor values. Because of this 
inference, the state of the cell is approximated. In an ideal world, the cell could only take 
on one of two values. The cell would either be empty or would be occupied. However, 
in practice, the sensors cannot provide the definitive state of the cell. Therefore, the 
probability of the state of each cell is recorded. The determination of the probability of 
the state of the cell given the current reading of the sensor is found using Bayes theorem 


Devore[14] defines Bayes Theorem in the following manner, ‘Let A), Ao, ...Ax be a 
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collection of k mutually exclusive and exhaustive events with prior probabilities 
P(A) G=1,...,.k). Then for any event B for which P(B)>0, the posterior probability of Aj 
given that B has occurred is 


P(A,OB)__ P(BIA,)P(A)) 4-2 


P(A, |B) = — 
ae) y( B| A,)e P(A,) 


Utilizing Bayes theorem will provide the probability of the state of the cell based 
on the current measurements only. What is needed is the state of the cell based on the 
current and all previous measurements. Elfes [16] provides an iterative solution to 
determining the probability that a cell is occupied given all previous measurements 


(Equation 4-3). 


P[s(C)=Oeel{r}...]= Plr..|3(C)=Oce |* PI s(C) =Oce|{r}, | 


7 wn ls(C)]* PL s(C)I {7}, | 


r,,,: The current measurement 
s(C): State of the cell 


4-3 


{r} : All measurement up to time t 


Expanding the summation in the probability update equation yields Equation 4-4. 
From here, it is easy to see the four unknowns that must be determined for each update. 


The unknowns are listed in Table 7. 


P[r., i = sea = Occ at 4-4 
PL iia | s(C) = Empty |* P| s(C = Empty [{r oe : | s( C)=Occ |* P| s(C) = Occ |{r}, ] 





P| s(C) =Occ | {}.] = 
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P| s(C) =Oce|{ r} | Probability that a cell is occupied given 
the current and all previous measurements 
Pls (C) = Occ | {r} ] Probability that a cell is occupied given all 
the previous measurements 
Pls (C) = Empty | { r} | Probability that a cell is occupied given all 
a the previous measurements 














Pl a s(C) = Occ | Probability of receiving the current 
measurement given that the cell is 
occupied 





P]. el s(C) = Empty | Probability of receiving the current 
measurement given that the cell is empty 


Table 7. Definition of Terms 














Since the cells can only have two states, empty or occupied, 
P| s(C) = Empty | al can be substituted with1— P| s(C) = Occ | {r} | , reducing the 
number of values to be calculated for each cell. For example, assume a cell is an 
unknown state P[s(C ) = Occ | = 0.6 and a new measurement is received. Based on the 
measurement and the corresponding probability functions the values of 
P[r,|s(C)=Occ] and P[r,,|s(C)=Empty| are determined. Assume that 
Pl %e | s(C)= Empty | =0.2 and Pl ra, |s(C)= Occ | =0.4. From these values the 


probability of the cell being occupied is updated (Equation 4-5). 


P[ s(C)=Occ|{r}.,, | a Oe 


= =0.75 4-5 
0.2*(1—0.6) +0.4* 0.6 





The recursive nature of this problem requires a _ seed value for 


Pi s(C ) = Occ | {r} |. At time zero, or the time prior to any measurements, this value 


indicates all the information the vehicle has about its environment. When the vehicle has 
zero information about its environment the cells are initialized to 0.5. An initial value of 
0.5 indicates that the cells are equally likely to be occupied or empty. As more 
information is attained this value is updated. This value will constantly be evolving 


based on the current input from the sensor. 
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The next values needed are the probability of receiving a particular measurement 
value given the state of the cell. These probability distributions are individual to the type 
of sensor employed. Since the probability distribution is dependent on the sensor type, 
the distributions should be defined prior to vehicle operation. The probability 
distributions associated with the sensor can be determined either theoretically or 
experimentally. Whether distributions are derived theoretically or through 
experimentation, their values can be stored and easily referenced at run time. The sensor 
probability distributions for the FLS were derived experimentally. The methods used to 


derive them will be covered in Chapter IV. 


The typical occupancy grid update starts when a new measurement is received. 
The sensor location is correlated to an occupancy grid location. The current occupancy 
grid value is retrieved. Next, the measurement is converted into two _ separate 
probabilities, the probability of receiving the signal assuming that the location is empty 
and the probability of receiving the signal assuming the location is occupied. Utilizing 
Equation 4-4 the new value of the probability of the grid cell being occupied is calculated 


and stored. This process will repeat for every sensor measurement that is received. 


C. COMBINING MULTIPLE SENSORS 


The previous discussion only covered the case in which the grid was dependent 
on a single sensor. However, in most robotic applications, the vehicle will have multiple 
sensors. The vehicle may have many sensors of the same type or a multitude of different 
classes of sensors. It is through the combination of multiple sensors that a better 
understanding of the environment can be made. In order to take full advantage of all the 
sensors that the vehicle deploys, the occupancy grid map must account for all sensor 


values. 


The most accurate method of combination would be to use a superbayesian 
approach. In this method, all sensors probabilities would need to include the dependency 
on the other sensors utilized. For example, when utilizing two different sensors, S1 and 
S2, the probabilistic sensor model for Slmust also take into account any information 


provided by S2. The new probabilistic model for S1 would be the probability of 
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receiving the particular sensor value given that the cell is occupied and that S2 received a 
given sensor value. The same holds true for S2. This redefines the probabilistic model 


used for each sensor utilized. 


There are two distinct disadvantages associated with the method. Many 
unmanned vehicles are capable of changing the sensors they employ. A vehicle that has 
five different sensors would require 120 (5!) sensor models to account for all of the 
possible sensor configurations. This alone makes this type of implementation 
impractical. Assuming that all 120 sensor models are created, if any sensor is added or 
upgraded all the previous sensor models are superseded. On today’s vehicles that are 


capable of rapid changes in sensor configuration, this method is not practical. 


An alternative method is to maintain separate occupancy grids for each sensor and 
then combine the grids utilizing an independent opinion pool. This method has proven 
successful by Elfes in [15]. In this scenario, each sensor will generate its own occupancy 
grid values. Assume the vehicle is utilizing sensors, S1 and S2, which in turn generate 
occupancy grids Pl and P2. The grid probabilities can be combined, as shown in 
Equation 4-6. 

PP, 


Pooled Probability = 
PP, +(1-F)(1-B) 


4-6 





The individual occupancy grid values will be dependent only on the sensor input 
and its probabilistic model. Maintaining the independence between sensors allows for a 
singular sensor models to be created. For example, REMUS is also equipped with a side 
scan sonar. Defining independent models for side scan sonar and FLS allows for their 
inclusion or exclusion in the reconstruction process based on current task. For the 
previous vehicle with five sensor, this reduces the number of required sensor models 
from 120 to 5. With the fewer number of models required, it becomes practical to 


implement this algorithm on unmanned vehicles. 


D. SUMMARY 


Occupancy grids can organize multiple sensor inputs into a format that can be 


used for safe and accurate navigation. They do not require any prior knowledge of the 
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area and can easily be utilized in highly congested areas. Occupancy grids are also well 
suited to combine multiple sensor inputs. The ability to navigate without any prior 
knowledge and the ability to determine the environment from all sensor inputs is an ideal 


combination for UUVs. 
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V. RESULTS AND CONCLUSIONS 


A. RESULTS 


Before discussing the results, it is important to recall the objectives of the 
research. REMUS’s ability to avoid obstacles is dependent upon its information about 
the environment. Currently REMUS is utilizing two-dimensional representation of the 
environment for path planning and obstacle avoidance. This limited REMUS’s ability to 


navigate successfully in cluttered environments, like under a bridge. 


In 2007, REMUS was deployed in the Charles River and sent on a path 
underneath the Massachusetts Avenue Memory Bridge. Figure 19 is a picture of the 
Massachusetts Avenue Bridge. Figure 20 shows the path of the vehicle. The path is 
shown in blue and the red circles indicate edges of the pylons supported the bridge. The 
GPS position of the pylons were acquired by placing a handheld GPS unit in close 
proximity to the part of the pylons that were above the surface. A limited number of 
satellites were acquired while receiving the position due to the bridge occluding GPS 
satellite reception. Due to the limited number of satellites recetved, there may be errors 


associated with the GPS positions of the columns. 





Figure 19. | Massachusetts Avenue Bridge, Boston, MA, 
from [18] 
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Figure 20. Vehicle Path in the Charles River and under the Massachusetts Avenue 
Bridge 


As discussed in Chapter I, the vehicle would not be able to navigate successfully 
between the pylons and over the berm if it relied on obstacle avoidance and two- 
dimensional representations of the bridge. This can be seen by Figure 4. With the 
combination of horizontal and vertical FLS and occupancy grids, a path emerges. Figure 
21 shows the occupancy grid created. The grid was formed using eleven images; 6 
horizontal and 5 vertical. These were the total number of images available to the vehicle 
as it approached the bridge. Figure 21 is composed of small cubes. A solid black cube 
corresponds to a probability of occupancy of nine-tenths or greater. Cubes with a 
probability between five and nine tenths are increasing shades of red. Cubes with a 
probability of one half or less are not depicted. Cubes with values of one half or less 
indicate unknown areas or areas not likely to be occupied. These cubes are not 


displayed. 
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Comparing Figure 21 with Figure 4 the success of occupancy grids readily stands 
out. In the horizontal image of Figure 4, the four berms were apparent. The problem 
with the horizontal view was that the berm appeared as a solid wall. In Figure 21, the 
central section of the wall created by the bern has a reduced height. This section is the 
area ensonified by the vertical FLS. The vertical FLS has lowered the height of the berm 
to its correct value. The vehicle can now recognize the path through the berm. A similar 
effect happens with the space between the pylons. Due to the ambiguity of the vertical 
FLS, the columns formed a vertical wall that covered the entire field of view. However, 
once combined with the horizontal FLS information, the edges and dimensions of the 


columns become easily identifiable. 





Path 






-300 
-350 
-400 


“200 '-450 


Figure 21. | Occupancy Grid Developed with 11 consecutive images (6 Horizontal, 5 
vertical) with the vehicle on a southwest approach 


Successfully locating a path for the vehicle is just part of the problem; the vehicle 


must also be able to correctly place the path into the global space. To determine the 
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accuracy of the generated map, the positions of the generated pylons are compared to 


measured values. The GPS location of the pylons was determined with a hand held GPS 


unit. Figure 22 shows an aerial view of the occupancy grid with the GPS measurements 


overlaid in green circles. The distance between the GPS position and the closest gridded 


pylon location were calculated. The results are listed in Table 8. 











Occupancy Grid and Column Locations 


Figure 22. 
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Actual Cell Location Distance (m) 
(-294.1, 315.2) (-296, -315) 1.9 
(-292.7, -317.1) (-296, -316) 305 

(314.6, -320) (-314. -320) 0.6 
(-288.6, -350.4) (-289, -350) 0.6 
(-283.1, -350.4) (-284, -349) Li 
(-274.9, -367.1) (-276. -367) 1.1 
(-294.1, -367.1) (-291, -369) 3.6 
(-261.2, -389.3) (-264, -397) 8.2 

(-259.8, -393) (-267, -396) 7.8 

(-279, -293) (-273, -298) 7.8 
(-283.1, 396.8) (-281, -401) 4.7 

(-279, -400.5) (-279, -401) 0.5 

















Table 8. Actual Pylon Location versus Cell Location 


When analyzing this data, a couple of observations become apparent. There is a 
significant offset between the location and gridded location of the lower pylon. The 
possible sources of error will be discussed in the following section. Excluding the lower 
most pylon, the rest of the data shows a mean distance error of 1.8 m. This error is much 


larger than expected with the high-resolution capabilities of the FLS. 


B. SOURCES OF ERROR 


Several factors play a role in the overall error distance. One of the main 
contributing factors is the grid size. The grid size (Im x 1m x 0.5m) was chosen to 


reduce the number of computations required. The grid size also provided enough 
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resolution for the vehicle to determine a path through under the bridge. However, this 


large grid size does not lend itself to a direct relationship with the sensor resolution. 


Another contributing factor is the method used for pixel probability placement 
into the grid. A single grid cell will have multiple pixels plotted to it for a single update. 
In this experiment, the highest probability values were placed within the cell. This 


method disregards information provided by the range of pixel contained in the cell. 


The final contributing factor to the error is the vehicle position and orientation at 
the time of the image. There are two separate contributors to this one source of error. 
First, the image data is not sampled at the same time or by the same computer as the 
vehicle state information. Because of this, the vehicle state information was interpolated 
to determine an approximate value at the time of the image. Any error in the state 
information is propagated through the algorithm and results in displaced detections. The 
second contributor to errors in the vehicle state is the approximations made by the 
internal navigation unit. The vehicle received a position fix at the beginning of the 


mission but was unable to receive any more fixes for the duration of the mission. 


Cc. RECOMMENDATIONS 


The research has demonstrated the proof of concept of utilizing dual FLS with 
occupancy grids to reconstruct a three-dimensional environment. However, progress 
needs to be made before the process is deployable on a vehicle in real time. Some of the 


areas that need further investigation are detailed below. 


1. Probabilistic FLS Model 


The validity of the model created for this experiment is limited to use in the 
Charles River. The model does not take into account the variability of many of the 
sources of noise. Many factors can affect the noise distributions. To be truly useful the 
noise distribution will need to take into account changes in the vehicles operational 
parameters. For example, how does the noise change as the speed of the vehicle 
changes? Does flow noise contribute significantly? What is the correlation in the noise 


model and the acoustic noise in the environment? Many parameters need to be analyzed 
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to develop a robust noise model. The detection distribution utilized for the FLS model 
will also need refinements. It is recommended that specific experiments be undertaken to 


provide a better classification of noise and detection distributions. 


2 Algorithm Improvements 


The algorithm in its current form is too slow to implement on an operational 
vehicle. An update based on a single image requires approximately 60 seconds to 
process. This is unacceptable for real time operation. It is recommended that the 
algorithm be transferred from a Matlab, a prototype environment, to a language designed 
for speed. One possible source of improvement is to down sample the FLS images. The 
current images provide a bearing resolution of 0.044 degrees. The actual minimum 
resolution of the FLS is specified as one degree. If the images are re-sampled in a 
manner to minimize the data lost, the number of computations can be drastically reduced. 
One possible method that can be used to resample the images is to use a Markov Random 
Field to determine the relationship among the pixels. Once a viable method for 
resampling has been found, the corresponding distribution functions will need to be 


created. 


3. Combine the Occupancy Grid Route Planning Algorithm 


The final recommendation is to couple this algorithm with route planning. The 
main goal of this thesis is to provide the vehicle a method to determine safe routes for 
navigation in a cluttered environment. While this thesis has shown a feasible method, 


until it is coupled with a route planning algorithm, its full utility cannot be determined. 


D. CONCLUSIONS 


There are issues to be addressed and further research needs to be conducted, but 
this research can stand as a proof of concept. It is possible to provide UUVs with a 
reconstructed three-dimensional representation of the underwater environment. This 


representation can then be used for route planning and obstacle avoidance. Using a 


Si 


three-dimensional reconstruction provides UUVs with the capability of successfully 
navigating in cluttered environments. With this capability, it becomes possible to safely 


deploy UUVs into more complex environments. 
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APPENDIX: 





























MATLAB CODE 








A. MERGING VEHICLE AND SONAR IMAGE DATASETS 
dataset = zeros(8100,15); 
% dataset = [elapsed time, lat, lon, x, y, depth, altitude, heading, pitch, 
z roll, velocity, headingRate, Horizontal Image #, Vertical 
Image #] 
% Original Data located in VehicleData, VertData, HorData 
SVehicle Elapsed time AtBeginning of first turn: 736 sec 
SSonar Elapsed time at Beginning of first turn: 685 sec 
offset = 736-685 
qe = Ly 
hdl = zeros(1,15); 
hd2 = hdl; 
vdl = zeros(1,15); 
vd2 = vdl; 
for i = 35:3000 
SRetrieve time of Horizontal FLS 
Htime = HorizData(i,1); 
SRetrieve time of Vertical FLS 
Vtime = VertData(i,1); 
SGet Vehicle State Associated with Horizontal FLS and Interpolate 
Hn = find(VehicleData(:,9)<(Htimetoffset), 1, 'last')-1; 
hdl = [VehicleData(Hn,9), VehicleData(Hn, 10:11), 0, 
0,VehicleData(Hn,12:18)]; 
Hn = Hnt+1; 
hd2 = [VehicleData(Hn,9), VehicleData(Hn, 10:11), 0, 
0,VehicleData(Hn,12:18)]; 
intp = (hd2(1)-(Htimetoffset) )/(hd2(1)—-hdl1(1)); 
[Hn, VehicleData(Hn-1l, 9), Htimetoffset, VehicleData(Hn,9), intp] 
hd = hdl + intp* (hd2-hdl); 
SGet Vehicle State Associated with Vertical FLS and Interpolate 
Vn = find(VehicleData(:,9)<(Vtimetoffset), 1, 'last'); 
vdl = [VehicleData(Vn,9), VehicleData(Vn, 10:11), 0, 
0,VehicleData(Vn,12:18)]; 
Vn = Vnt+1; 
vd2 = [VehicleData(Vn,9), VehicleData(Vn, 10:11), 0, 


0,VehicleData(Vn,12:18)]; 
intp 
vd2 


vdltintp* (vd2-vdl); 





(vd2 (1) -(Htime+offset) ) / (vd2 (1) -vdl1 (1) ) 


SStore combined data to a new dataset ordered by offset time 


switch logical (true) 





case (Htime<=Vtime) 
dataset (j,:) = [hd2, i, i-1, 0]; 
dataset (jtl, :) = [vd2, i, i, 0]; 
j= 4+2; 

case (Htime>Vtime) 
dataset (j,:) = [vd2, i-1, i, 0] ; 
dataset (jtl, :) = [hd2, i, i, 0]; 
j= +2; 


end 
end 


SConvert GPS to meters in a local grid 
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% Reference point (42.358301, -71.088055) 





for i = 1:size(dataset,1) 


end 


lat = dataset(i, 2); 
lon = dataset (i,3); 

dataset(i,4) = 

dataset (i,5) = cos (2*pi*lat/360) *60*1853.47* (lon--71.088055); 


(lat-42.358301) *60*1853.47; 


clear hdl hd2 vdl vd2 intp i Vn Hn Htime Vtime j lat lon 
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% clear VehicleData VertData HorizData 





GENERATING FLS PROBABILITY MODEL 


bins = linspace(0,4000,25); 
binstorage = zeros(size(bins)); 


SGenerate Noise Distributions 


binstorage = zeros(461, 2048, numel(bins)); 
for i = 300:20:600 %Range of Images involved in Noise Distribution 


SCombine 20 Images 
for j = 1:20 








imageData(:,:, Jj) = OpenHorSonarImage('..\SonarImages\8_bit', 
MPs hor.) 4, -' png! 7. Tt q=1): 
end 
m = zeros(size(binstorage) ); 
SCreate Histograms 
for k = 1:size(imageData, 1) 
for p = l:size(imageData, 2) 
m(k,p,:) = hist (imageData(k,p,:),bins); 
end 
end 
binstorage = binstoraget+m; 
id=i 


clear imageData 
end 
clear ijkpmbl 
SSave Histogram as Noise Histogram 


Vnoise = binstorage; 


sGenerete Sensor Histograms 
imageData = zeros(461, 2048, 20); 


binstorage = zeros(461, 2048, numel(bins)); 
SImage Ranges used in Dataset 
T_range = [820, 960; 1125, 1265; 1505, 1617; 1840, 1910]; 


for 1 = 1:4 
start = I_range(1,1); 
stop = I_range(1,2); 
for i = start:20:stop 
for j = 1:20 
imageData(:,:, Jj) = OpenHorSonarImage('..\SonarImages\8_bit', 
Vis. NOrz oy 4, '.png', itj); 
end 


m = zeros(461, 2048, numel(bins)); 
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for k = 1:size(imageData, 1) 








for p = l:size(imageData, 2) 
m(k,p,:) = hist (imageData(k,p,:),bins); 
end 
end 
binstorage = binstoragetm; 
id=i 


end 
end 
SSave Sensor Distribution 
Vsensor = binstorage; 
Vbins = bins; 
clear imageData binstorage I_range start stop 
clear ijmkop 
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SRemove Noise From Sensor Distribution 


n = zeros(size(Vbins)); 
s =n; 

t =n; 

i = 200; 3 = 880; 





SUpdate the Distributions for Each Pixel 
for i= 1:461 


% for j = 1:2048 
SRetrieve Noise Histogram 
n(:) = Hnoise(i,j,:); 


figure (4); subplot (4,2,1); bar(Hbins, n); title('Noise 
Distribution") 
SNormalize Noise 
n = n/sum(n); 
[mn, mi] = max(n); 
figure (4); subplot (4,2,2); bar(Hbins, n);title('Normalized Noise 
Distribution, n') 
% Retieve Sensor Histogram 
s(:) = Hsensor(i,j,:); 
figure (4); subplot (4,2,3); bar(Hbins, s); title('Detection 
Distribution") 
% Normalize sensor over range of Noise Distribution 
s = s/sum(s.*(n>0)); 
figure (4); subplot (4,2,4); bar(Hbins, s); title('Normalized 
Detection Distribution, s') 
SRemove Noise and Ensure all values are positive, then Normalize 





Ss = s-n; 
% figure (4); subplot (4,2,5); bar(Hbins, s); title('s-n') 
s= s + abs(min(s.*(s<0))); 
s = s/sum(s); 


figure (4); subplot (4,2,6); bar(Hbins, s); title('(s-n) 

raised to a min of zero') 
% s = s+0.001*abs (min(s+10.* (s==0))); 

figure(4); subplot (4,2,7); bar(Hbins, (s+n)/sum(s+tn)); 
title('Adjusted Detection Distrobution') 

s = (1-s).*(s~=0); 

s = s/sum(s); 

figure (4); subplot (4,2,8); bar(Hbins, (stn) /sum(stn)); 
title('Adjusted Detection + Noise') 

n = ntmax(min(nt+10.* (n==0)),0.01); 


ole 


Save Adjusted Distribution to Sensor and Noise Models 
Hnoise(i,j,:) = n/sum(n); 
Hsensor(i,j,:) =s; 
end 
end 


55 























GENERATING THE MAP 
1. Main Function 
SESTABLISH THE BOUNDARIES OF THE GRID 
be= [0 0..25:-505 0. +257 50% .0.-0.2425- 10]; 
b(1,1:3) = b(1,1:3) + -300; 
b(2Z, 13) ‘= b(2, 113) + -385; 
bel 720 2) = 08257 
for i= 1:3 
b(i,4) = numel(b(i,1):b(i,2):b(i,3)); 
end 
b=b 


SInitialize all variables 
SImage Containers 
imgH = zeros(461, 2048 


); 


) 
imgS = zeros (size (imgH) 
imgV = zeros(461, 1024); 
imgSv = zeros(size(imgV)); 


SGrid Containters 

gH = 0.5*ones(b(1,4), b(2,4), b(3,4)); 
gV = 0.5*ones(size(gH)); 

gN zeros (size(gH) ); 

gs zeros (size(gH) ); 

gM = 0.5*ones(size(gH)); 


SVehicle State 
vd = zeros(1,6); 





HIN = 0; 
VIN = 0; 
dr = [1662, 1672 ; 2280, 2290 ] %Establish images be considered 
for 1 =1 
for vdn = dr(1,1):dr(1,2) 
tic 
SExtract Vehicle state and Images Numbers 
vd = [dataset(vdn, 5), dataset(vdn, 4), dataset(vdn, 7), 90- 
dataset (vdn, 8), dataset(vdn, 9), dataset(vdn, 10)]; 
% Generate Horizontal Map 


if HIN < dataset (vdn, 13) 
HIN = dataset (vdn, 13); 


imgH = OpenHorSonarImage('../SonarImages/RTheta', 'fls_horiz_', 
4, ‘'.pgm', HIN); SGet Images 

imgH (find(isnan(imgH) ))=2; 

imgS = ProbImages(imgH, Hsensor, Hbins); SDetection Prob 
Image 

img = ProbImages(imgH, Hnoise, Hbins); SNoise Prob Image 

gS = HorImage2Map(imgS, vd, b); SDetection Prob Grid 

gMask = gS>=0; SMask of updated 
Values 

gN = HorImage2Map(img, vd, b); SNoise Prob Grid 

SHorizontal Grid Update 

gS = (gS.*gH) ./ (gN.* (1-gH) +gH.*gS) ; 

gH = gS.*gMask+gH.* (~gMask) ; 

end 


ole 


Generate Vertical Map 
if VIN < dataset (vdn, 14) 
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VIN = dataset (vdn, 14); 

imgV = OpenVertSonarImage('..\SonarImages\RTHeta', 'fls_vert_', 
4, ‘'.pgm', VIN); SGet Image 

imgSv = ProbImages(imgV, Vsensor, Vbins) ; SDetection Prob Image 

imgV = ProbImages(imgV, Vnoise, Vbins); SNoise Prob Image 

gS = VertImage2Map(imgSv, vd, b); SDetection Prob Grid 

gMask = gS>=0; SGrid Mask 

gN = VertImage2Map(imgV, vd, b); SNoise Prob Grid 

% Vertical Grid Update 

gS = (gS.*gV) ./(gN.* (1-gV) +gS.*gV) ; 

gV = gS.*gMask+gV.* (~gMask) ; 


end 


ole 


Pooled UPdate 
gM = (gV.*gH) ./ (gH. *gV+(1-gV) .* (1-gH) ); 


[HIN, VIN, vdn] SShow Loop Progress 
end 
end 


2. Converting FLS Images to Probabilities 


function Prob = ProbImages (img, SensorDist, binSpacing) 

SThis function creates to probability images based upon the 
provided image 

Sand the Sensor Probability Distribution (modeled as a histogram). 
Each 

Sbin contains the probability of recieving a sensor value within 
the edges 

Sof the bin. 





ole 


% SInputs: 

% img: Sensor Image Mx N 

% binSpacing: Vector indicating the edges of the probability 
bins (length P) 

% SensorDist: Probability Distribution of the sensor. 

% Mx N x P 

% Outputs: 

% Prob: An M x N matrix wher ach element represents th 





probability of receiving each pixel 


ole 


Prob = 0.5*ones(size(img) ); 





STraverse th ntire imag 
for j = 1l:size(img, 2) 
for i = 1:size(img,1) 
SFind out which Bin number the intensity falls into 
binNum = find(img(i,j)<binSpacing, 1, 'first'); 
if binNum >0 
SRetrieve probability of receiving that intensity value 


Prob(i,j) = squeeze (SensorDist (i, j,binNum) ); 
else 
Prob(i,j) = 0.0001; %Assign a minimum Probability 
end 
end 


end 
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Mapping Image Data to Grid Locations 


a. Horizontal FLS 


function imageMap = HorImage2Map(img,VehicleData, bounds) 


ole 


Sbounds 
[ xmin, xres, xmax, # of cells 
ymin, yres, ymax, # of cells 
zmin, zres, zmax, # of cells ] 
temp = []; 
ent = 1; 


AP ol? 


ole 


ambspacing = 0.5*pi/180; 
ambend = 7*pi/180; 


SSet Vehicle position and orientation parameters 


x0 = VehicleData(l1); SLocalX 
yO = VehicleData(2); *tLocalY 
zO = VehicleData (3); sDepth 





heading = VehicleData (4) *pi/180; 
pitch = VehicleData(5) *pi/180; 
roll = VehicleData(6)*pi/180; 





SGen imageMap 

xr = bounds (1,1) :bounds (1,2) :bounds (1 

yr = bounds (2,1) :bounds (2,2) :bounds (2 

zr = bounds (3,1) :bounds (3,2) :bounds (3 
d 


imageMap = -—1*ones (bounds (1,4), boun ye), wounds (3,4) )3 





SEstablish Image Parameters 

imgH = size(img,1); 

imgW = size(img,2); 

SSpherical Coordinates 

theta = linspace(pi/4, -pi/4, imgW); 
r = linspace(90,0, imgH); 

phi = -ambend:ambspacing: ambend; 


for phil = 1:size(phi,2) 





[gr, gt, gphi ] = ndgrid(r, theta, phi(phil)); 
SConvert data from Local Spherical to Global Cartesian 
[x, y, z] = sph2cart(gttheading, gphit+pitch, gr); 
SMove Image Data to Vehicle Location 

x = x+x0; 

y = yty0; 

Z = z+z0; 

SCovert Image Position to Grid Values 

xi = ceil( (x-bounds(1,1))/bounds(1,2)); 

xi = ((xi>0)&(xi<xsize(xr,2))).*xi; 

yi = ceil( (y-bounds(2,1))/bounds(2,2)); 

yi = ((yi>0)&(yi<size(yr,2)-1)).*yi; 

zi = ceil( (z-bounds (3,1))/bounds (3,2) ); 

Zi = ((zi>0)&(zi<size(zr,2)-1)).*zi; 


SStore Image Values into Grid Position 
for rI = 1:imgH 
for thetaI = l:imgW 
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SDetermine if image value is contained within map 
valid = (xi( ri, thetaI, 1)>0 & yi( rI, thetaI, 1)>0 & 
zi( rI, thetaI, 1)>0); 
SIf Valid Store Data contain in image pixel to 
corresponding Grid 
SLocations 
if valid 
val = imageMap(xi( rI, thetalI, 1), yi( rl, thetal, 
Lies Za. £L,- thetaL,;-1)).3 
imageMap(xi( rI, thetaI, 1), yi( rl, thetalI, 1), 
zi( rI, thetalI, 1)) = max(img(rI, thetalI), val); 
end 


end 


end 
end 


b. Vertical FLS 


function imageMap = VertImage2Map (img, VehicleData, bounds) 
% Sbounds 





% [ xmin, xres, xmax 

% ymin, yres, ymax 

% zmin, zres, zmax ] 
temp = []; 

ent = 1; 


ambspacing = 0.5*pi/180; 
ambend = 7*pi/180; 


SSet Vehicle position and orientation parameters 
x0 = VehicleData(1); LocalX 

yO = VehicleData (2); LocalY 

zO = VehicleData(3); Depth 

heading = VehicleData (4) *pi/180; 

pitch = VehicleData(5) *pi/180; 

roll = VehicleData (6) *pi/180; 


AP al lO 


nS 








SGen imageMap 














xr = bounds (1,1) :bounds (1,2) :bounds (1,3); 
yr = bounds (2,1) :bounds (2,2) :bounds (2,3); 
zr = bounds (3,1) :bounds (3,2) :bounds (3,3); 
imageMap 1*ones (size (xr, 2) size(yr,2), size(zr,2)); 
imgH = size(img,1); 
imgW = size(img,2); 
phi = linspace(-pi/16, pi/16, imgW); 
vr = linspace(90,0, imgH); 
theta = -ambend:ambspacing: ambend; 
for thetaI = 1:numel (theta) 
[gr, gt, gphi ] = ndgrid(r, theta(thetal), phi); 
[x,y,z] = sph2cart(gtt+theading, gphitpitch, gr); 
x = x+x0; 
y = yty0; 
Z = z+z0; 
xi = ceil( (x-bounds(1,1))/bounds(1,2)); 
xi = ((xi>0)&(xi<xsize(xr,2))).*xi; 
yi = ceil( (y-bounds(2,1))/bounds(2,2)); 
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yi = ((yi>0O)&(yi<size(yr,2)-1)).*yi; 
zi = ceil( (z-bounds (3,1))/bounds (3,2)); 
Zi = ((zi>0)&(zi<size(zr,2)-1)).*zi; 


ol? 


size (imageMap) 
[size(xr, 2), size(yr,2) ;max(max(max(xi))), 


ole 


min(min(min(xi))); max(max(max(yi))), 
min(min(min(yi)));max(max(max(zi))), min(min(min(zi)))] 
for rl = 1l:imgH 
for angle = l:imgW 

% temp = [xi(rI, thetaI, 1), yi(rlI, thetalI, 1),zi(rI, 
thetal, 1)] 

valid = (xi( rI, 1, angle)>0 & yi( rI, 1, angle)>0 & zi( 
rI, 1, angle)>0); 

if valid 

val = imageMap(xi( rI, 1, angle), yi( rI, 1, 


angle), zi( rI, 1, angle)); 
imageMap(xi( rI, 1, angle), yi( rI, 1, angle), zi( 
rI, 1, angle)) = max(img(rI, angle), val); 
end 


end 

end 
end 
SSet portions of the map without data to a value of .5 (unknown 
state). 
% imageMap = (imageMap <0).*.5 + imageMap.* (imageMap>=0) ; 
% imageMap = reshape(imageMap, bounds (4,1) *bounds (4,3), 
bounds (4,2)); 
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4. Display the Grid 


function drawGrid(gridMap, b, axis_handle) 
SGeneric Cube of Cell Dimensions 

xcube = b(1,2)* [ 00110 NaN 01 NaN 1 O;... 
0 NaN 0 1 NaN 1 O]; 





xcube = xcubetb(1,1) 
ycube = b(2,2)* [ 0110 0NaN 11 NaN 1 1;... 
O 1100 NaN 0 0 NaN O 0]; 
ycube = ycubetb (2,1) 
zcube = b(3,2)* [ 1 1111 NaN 11 NaN 0 0O;... 
000 0 0 NaN 11 NaN O 0]; 
zcube = zcube +b(3,1); 
STraverse entire Grid, Draw cubes based on the Probability contained in 
Sthe cell 
for i = l:size(gridMap, 1); 
for j = l:size(gridMap, 2); 
for k = l:size(gridMap, 3); 























switch logical (true) 
case gridMap(i,j,k) >0.95 
surf (xcubeti*b(1,2), ycubet+j*b(2,2), zcubetk*b(3,2), 

"facecolor', 0 0 0], 'edgecolor', 'none'); 
case gridMap(i,j,k) >= 0.8 

surf (xcubeti*b(1,2), ycubetj*b(2,2), zcubetk*b(3,2), 
"facecolor', 0.25 0 0], 'edgecolor', 'none' ); 
case gridMap(i,j,k) > 0.7 

surf (xcubeti*b(1,2), ycubet+j*b(2,2), zcubetk*b(3,2), 
"facecolor', 0.5 0 0], 'edgecolor', 'none'); 
case gridMap(i,j,k) > 0.5 

surf( xcubeti*b(1,2), ycubetj*b(2,2), zcubet+k*b(3,2), 
"facecolor', 0.75 0 0], 'edgecolor', 'none'); 





























end 
hold on 
end 
end 
if mod(i,10) == 0 
i=i 
end 


end 
axis([ b(1,1) b(1,3) b(2,1) b(2,3) b(3,1) b(3,3)]);7 
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